﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Windows.Forms;

using MotionPlanning.MotionPlanningPrimitives;
using MotionPlanning.MotionPlanningAlgorithms;

namespace MotionPlanning.MotionPlanningComponents.Visualizations
{
    public class Visualizer : VisualizationComponent
    {
        protected Graphics canvas;
        protected Rectangle canvasRect;
        protected SolidBrush brushCanvasClear;

        protected int center_x;
        protected int center_y;
        protected double calibrater_y;
        protected double calibrater_x;
        protected int screen_width;
        protected int screen_height;

        protected Planner_RRTBased planner_subject;
        protected double cost_cutoff_low;
        protected double cost_cutoff_high;

        protected float pointRadius;
        protected Color color_low;
        protected Color color_high;

        protected Pen penEdge;
        protected Pen penState;
        protected Pen penBlue;
        protected Pen penRed;
        protected Pen penGreen;
        protected Pen penWhite;
        protected Pen penBlack;
        protected Pen canvasAxisGray;

        protected SolidBrush brushEdge;
        protected SolidBrush brushState;
        protected SolidBrush brushCanvasAxis;
        protected SolidBrush brushBlue;
        protected SolidBrush brushRed;
        protected SolidBrush brushGreen;
        protected SolidBrush brushWhite;
        protected SolidBrush brushBlack;

        protected Color[] ColorBeltArray;

        public Visualizer(Graphics canvas, Rectangle canvasRect)
        {
            this.canvas = canvas;
            this.canvasRect = canvasRect;
            calibrater_x = canvasRect.Width / 2;
            calibrater_y = -canvasRect.Height / 2;
            center_x = canvasRect.Width / 2;
            center_y = canvasRect.Height / 2;
            screen_width = canvasRect.Width;
            screen_height = canvasRect.Height;

            cost_cutoff_low = 0;
            cost_cutoff_high = 10;

            //------------------------
            color_high = Color.Black;
            color_low = Color.Yellow;
            pointRadius = 1;

            penEdge = new Pen(Color.Blue);
            penState = new Pen(Color.Red);
            canvasAxisGray = new Pen(Color.Gray);
            penBlue = new Pen(Color.Blue);
            penRed = new Pen(Color.Red);
            penGreen = new Pen(Color.Green);
            penWhite = new Pen(Color.White);
            penBlack = new Pen(Color.Black);

            brushEdge = new SolidBrush(Color.Blue);
            brushState = new SolidBrush(Color.Red);
            brushCanvasAxis = new SolidBrush(Color.Gray);
            brushCanvasClear = new SolidBrush(Color.White);
            brushBlue = new SolidBrush(Color.Blue);
            brushRed = new SolidBrush(Color.Red);
            brushGreen = new SolidBrush(Color.Green);
            brushWhite = new SolidBrush(Color.White);
            brushBlack = new SolidBrush(Color.Black);

            ColorBeltArray = new Color[]{
                Color.Red,
                Color.Orange,
                Color.Magenta,
                Color.Green,
                Color.Cyan,
                Color.Blue,
                Color.Purple
            };
        }
        public Rectangle GetCanvasSize()
        {
            return canvasRect;
        }
        public void SetCanvas(Graphics canvas)
        {
            this.canvas = canvas;
        }
        virtual public void ClearCanvas()
        {
            canvas.FillRectangle(brushCanvasClear, canvasRect);
            canvas.DrawLine(canvasAxisGray, 0, center_y, screen_width, center_y);
            canvas.DrawLine(canvasAxisGray, center_x, 0, center_x, screen_height);
        }
        virtual public void RenderFrame()
        {
            ClearCanvas();
            PlotVerticesWithCost(planner_subject.GetVertices().GetAllDataObjects());
            canvas.DrawLine(canvasAxisGray, 0, center_y, screen_width, center_y);
            canvas.DrawLine(canvasAxisGray, center_x, 0, center_x, screen_height);
        }
        virtual public void PlotVerticesWithCost(List<object> train)
        {
            double dotRadius = 2.5;
            double res = 2;
            double tol = 0.99;
            for (int i = 0; i < train.Count; ++i)
            {
                Node cn = ((Node)train[i]);

                double cost = cn.get_Value().get_utility_c();
                double[] q1 = cn.get_Value().get_q();

                int screen_x1 = (int)(q1[0] * this.calibrater_x + center_x);
                int screen_y1 = (int)(q1[1] * this.calibrater_y + center_y);

                double cn_cost = cn.get_Value().get_utility_c();

                if (cn_cost > ColorBeltArray.Length * res) continue; // comment out this line to see all black points

                int intg = (int)Math.Floor(cn_cost / res);
                double normalizedResidue = (cn_cost % res) / res;
                SolidBrush vertexBrush = null;
                if (normalizedResidue < tol)
                {
                    Color color = Color.Black;
                    if (intg < ColorBeltArray.Length)
                        color = ColorBeltArray[intg];
                    vertexBrush = new SolidBrush(color);
                }
                else
                {
                    continue;
                    //vertexBrush = new SolidBrush(Cost2Color(cn_cost, 30));
                }

                canvas.FillEllipse(vertexBrush, (float)(screen_x1 - dotRadius), (float)(screen_y1 - dotRadius), (float)(dotRadius + dotRadius), (float)(dotRadius + dotRadius));

            }
            canvas.DrawLine(canvasAxisGray, 0, center_y, screen_width, center_y);
            canvas.DrawLine(canvasAxisGray, center_x, 0, center_x, screen_height);
            /*
            double[] q_root = planner_subject.GetRoot().get_Value().get_q();
            int x_start = (int)(center_x + q_root[0] * calibrater_x);
            int y_start = (int)(center_y + q_root[1] * calibrater_y);
            double radius_i = 0;
            for (int i = 0; i < ColorBeltArray.Length; ++i)
            {
                radius_i += res;
                canvas.DrawEllipse(new Pen(Color.Red),
                    x_start - (int)(radius_i * calibrater_x),
                    y_start - (int)(radius_i * calibrater_y),
                    (int)(radius_i * calibrater_x * 2),
                    (int)(radius_i * calibrater_y * 2));
            }
             */
        }
        public void PlotVerticesWithDuration(List<object> train, long T_near, long T_far)
        {

        }
        virtual public void SetHostPlanner(Planner_RRTBased host)
        {
            planner_subject = host;
            double[] q_max = planner_subject.GetSystem().Get_q_max();
            double[] q_min = planner_subject.GetSystem().Get_q_min();
            double range_x = (q_max[0] - q_min[0]) / 2;
            double range_y = (q_max[1] - q_min[1]) / 2;
            this.calibrater_x = canvasRect.Width / 2 / range_x;
            this.calibrater_y = -canvasRect.Height / 2 / range_y;
        }
        public void VisualizeState(double[] q_vis)
        {
            throw new NotImplementedException();
        }
        virtual public void PlotVertices(List<object> train)
        {
            for (int i = 0; i < train.Count; ++i)
            {
                double[] cq = ((Node)train[i]).get_Value().get_q();
                int screen_x = (int)(cq[0] * this.calibrater_x + center_x);
                int screen_y = (int)(cq[1] * this.calibrater_y + center_y);
                canvas.FillEllipse(brushState, screen_x - pointRadius, screen_y - pointRadius, pointRadius + pointRadius, pointRadius + pointRadius);
            }
        }
        virtual public void PlotEdges(List<object> train)
        {
            for (int i = 0; i < train.Count; i += 1)
            {
                List<Edge> pe = ((Node)train[i]).get_Parents();
                if (pe == null) continue;
                Edge p = pe[pe.Count - 1];
                double[] q1 = p.get_entry().get_Value().get_q();
                double[] q2 = p.get_exit().get_Value().get_q();

                int screen_x1 = (int)(q1[0] * this.calibrater_x + center_x);
                int screen_y1 = (int)(q1[1] * this.calibrater_y + center_y);
                int screen_x2 = (int)(q2[0] * this.calibrater_x + center_x);
                int screen_y2 = (int)(q2[1] * this.calibrater_y + center_y);

                canvas.DrawLine(penEdge, screen_x1, screen_y1, screen_x2, screen_y2);
            }
        }
        public void PlotEdges(List<Edge> train)
        {
            if (train == null)
                return;
            for (int i = 0; i < train.Count; i += 1)
            {
                double[] q1 = train[i].get_entry().get_Value().get_q();
                double[] q2 = train[i].get_exit().get_Value().get_q();

                int screen_x1 = (int)(q1[0] * this.calibrater_x + center_x);
                int screen_y1 = (int)(q1[1] * this.calibrater_y + center_y);
                int screen_x2 = (int)(q2[0] * this.calibrater_x + center_x);
                int screen_y2 = (int)(q2[1] * this.calibrater_y + center_y);

                canvas.DrawLine(penEdge, screen_x1, screen_y1, screen_x2, screen_y2);
            }
        }
        virtual public void PlotTrajectory(Trajectory trajectory)
        {
            List<Node> nodeArray = trajectory.GetNodes();
            List<object> train = new List<object>(nodeArray.Count);
            for (int i = 0; i < nodeArray.Count; ++i)
            {
                train.Add(nodeArray[i]);
            }
            PlotEdges(train);
            PlotVertices(train);
        }
        protected Color Cost2Color(double cost, double cutOffLow, double cutOffHigh)
        {
            if (cost > cutOffHigh)
                return color_high;
            if (cost < cutOffLow)
                return color_low;
            if (cutOffLow >= cutOffHigh)
                throw new Exception("cutOffLow should be less than cutOffHigh");
            double relativeRatio = (cost - cutOffLow) / (cutOffHigh - cutOffLow);
            return Color.FromArgb(0, (int)(255 - 255 * relativeRatio), 0);
        }
        public float get_pointRadius()
        {
            return pointRadius;
        }
        public void set_pointRadius(float pointRadius)
        {
            this.pointRadius = pointRadius;
        }
        public void set_color_low(Color color)
        {
            this.color_low = color;
        }
        public void set_color_high(Color color)
        {
            this.color_high = color;
        }
        public void set_cost_cutoff_low(double value)
        {
            cost_cutoff_low = value;
        }
        public void set_cost_cutoff_high(double value)
        {
            cost_cutoff_high = value;
        }
    }
}
